/*
 * Copyright 2011 Gedare Bloom.
 *
 * The license and distribution terms for this file may be
 * found in the file LICENSE in this distribution or at
 * http://www.rtems.com/license/LICENSE.
 *
 * $Id$
 *
 */

#include <rtems/asm.h>

#include <arch/boot.h>
#include <arch/stack.h>

#include <arch/mm/mmu.h>
#include <arch/mm/tlb.h>
#include <arch/mm/tte.h>

#define PHYSMEM_ADDR_SIZE 43


#define SET_TLB_TAG(VA, r1, context) \
	set VA | (context << TLB_TAG_ACCESS_CONTEXT_SHIFT), %r1

#define TTE_LOW_DATA(imm) 	(TTE_CP | TTE_P | LMA | (imm))

#define SET_TLB_DATA(r1, r2, imm) \
	set TTE_LOW_DATA(imm), %r1; \
	or %r1, %l5, %r1; \
	mov PAGESIZE_512K, %r2; \
	sllx %r2, TTE_SIZE_SHIFT, %r2; \
	or %r1, %r2, %r1; \
	mov 1, %r2; \
	sllx %r2, TTE_V_SHIFT, %r2; \
	or %r1, %r2, %r1;

#define SET_TLB_DEMAP_CMD(r1, context_id) \
  set (TLB_DEMAP_CONTEXT << TLB_DEMAP_TYPE_SHIFT) | (context_id << \
    TLB_DEMAP_CONTEXT_SHIFT), %r1

.register %g2, #scratch
.register %g3, #scratch

.section BOOTSTRAP

/* pass o0 as start of physical memory */
.global _take_mmu
_take_mmu:
	save %sp, -STACK_WINDOW_SAVE_AREA_SIZE, %sp
  mov %i0, %l6
  
  /* these are copied from above */
  ! Get bits (PHYSMEM_ADDR_SIZE - 1):13 of physmem_base.
  srlx %l6, 13, %l5

  ! l5 <= physmem_base[(PHYSMEM_ADDR_SIZE - 1):13]
  sllx %l5, 13 + (63 - (PHYSMEM_ADDR_SIZE - 1)), %l5
  srlx %l5, 63 - (PHYSMEM_ADDR_SIZE - 1), %l5

  /* precompute mapping for 512K pages */
  sethi %hi(sparc64_512K_tlb_data_template), %l4
  ldx [%l4 + %lo(sparc64_512K_tlb_data_template)], %l3
  or %l3, %l5, %l3
  stx %l3, [%l4 + %lo(sparc64_512K_tlb_data_template)]

  /*
	 * Switch to kernel trap table.
	 */
	sethi %hi(trap_table), %g1
	wrpr %g1, %lo(trap_table), %tba

	/*
   * Install TTEs to identically map first 512K of memory.
	 */
	wr %g0, ASI_DMMU, %asi

  ! do identity map
  mov %l5, %l0
  mov %g0, %l5

  set _take_mmu, %g5

  ! First 512K
  SET_TLB_TAG(0x4000, g1, MEM_CONTEXT_KERNEL)
  stxa %g1, [VA_DMMU_TAG_ACCESS] %asi     
  membar #Sync
  SET_TLB_DATA(g1, g2, TTE_W) ! use non-global mapping
  stxa %g1, [%g0] ASI_DTLB_DATA_IN_REG    
  membar #Sync

	SET_TLB_TAG(0x4000, g1, MEM_CONTEXT_KERNEL)
	mov VA_DMMU_TAG_ACCESS, %g2
	stxa %g1, [%g2] ASI_IMMU
	flush %g5
	SET_TLB_DATA(g1, g2, 0)		! use non-global mapping
	stxa %g1, [%g0] ASI_ITLB_DATA_IN_REG		
	flush %g5

#if 0
  ! Second 512K
  SET_TLB_TAG(0x404000, g1, MEM_CONTEXT_KERNEL)
  stxa %g1, [VA_DMMU_TAG_ACCESS] %asi
  membar #Sync
  set 0x400000, %g1
  add %g1, %l5, %l5
  SET_TLB_DATA(g1, g2, TTE_W)
  stxa %g1, [%g0] ASI_DTLB_DATA_IN_REG
  membar #Sync
  ! IMMU
  SET_TLB_TAG(0x404000, g1, MEM_CONTEXT_KERNEL)
	mov VA_DMMU_TAG_ACCESS, %g2
	stxa %g1, [%g2] ASI_IMMU
	flush %g5
	SET_TLB_DATA(g1, g2, 0)		! use non-global mapping
	stxa %g1, [%g0] ASI_ITLB_DATA_IN_REG		
	flush %g5

  ! Third 512K 
  SET_TLB_TAG(0x804000, g1, MEM_CONTEXT_KERNEL)
  stxa %g1, [VA_DMMU_TAG_ACCESS] %asi
  membar #Sync
  set 0x400000, %g1
  add %g1, %l5, %l5
  SET_TLB_DATA(g1, g2, TTE_W)
  stxa %g1, [%g0] ASI_DTLB_DATA_IN_REG
  membar #Sync
  ! IMMU
  SET_TLB_TAG(0x804000, g1, MEM_CONTEXT_KERNEL)
	mov VA_DMMU_TAG_ACCESS, %g2
	stxa %g1, [%g2] ASI_IMMU
	flush %g5
	SET_TLB_DATA(g1, g2, 0)		! use non-global mapping
	stxa %g1, [%g0] ASI_ITLB_DATA_IN_REG		
	flush %g5
#endif

  /*
	 * Flush D-Cache.
	 */
	call rtems_cache_flush_entire_data
	nop

  setx sparc64_identity_map_dtlb, %l0, %o1
  setx param_space, %l0, %o2
  call SYM(_CPU_ISR_install_raw_handler)
  mov 0x68, %o0

  ret
  restore

/* This approach fails for firmware code: only use at tl0.
 * If too many tlb entries are loaded, firmware tlb entries are replaced
 * and the system dies. So map them in 512KB chunks!
 */
.global sparc64_identity_map_dtlb
sparc64_identity_map_dtlb:
  sethi %hi(fast_data_access_mmu_miss_data_hi), %g7
  wr %g0, ASI_DMMU, %asi
  ldxa [VA_DMMU_TAG_ACCESS] %asi, %g4
  set TLB_TAG_ACCESS_CONTEXT_MASK, %g2
  andcc %g4, %g2, %g3       ! get Context
  bnz %xcc, nonzero_context ! Context is non-zero
  andncc %g4, %g2, %g3                    ! get page address
  bz  %xcc, data_access_page_zero         ! spin (should fault) if page 0
  nop
  clr %g4
  ldx [%g7 + %lo(sparc64_512K_tlb_data_template)], %g2
  add %g3, %g2, %g2
  stxa %g2, [%g0] ASI_DTLB_DATA_IN_REG
  retry

data_access_page_zero:
  ba data_access_page_zero
  nop
nonzero_context:
  ba nonzero_context
  nop

past_ram_end: 
  ba past_ram_end
  nop

!.data
.align 16
.global fast_data_access_mmu_miss_data_hi
fast_data_access_mmu_miss_data_hi:
.global sparc64_physmem_base
sparc64_physmem_base:
  .quad 0

.global sparc64_512K_tlb_data_template
sparc64_512K_tlb_data_template:
  .quad ((1 << TTE_V_SHIFT) | (PAGESIZE_512K << TTE_SIZE_SHIFT) | TTE_CP | \
      TTE_P | TTE_W)

